#include "WPILib.h"
#include "Defs.h"
#include "nivision.h"
#include <iostream>
#include <fstream>
#include <cmath>
#include <string>
#include <sstream>
#include "Ballistic2D.h"

class Robot2489: public SimpleRobot{
	
	typedef enum {kTop, kBottom, kLeft, kRight} particlePos;
	typedef enum {kLeftSide, kRightSide} fieldSide;
	typedef struct particlePoint{
		particlePos position;
		double xCenter;
		double yCenter;
		double Height;
		double Width;
	};
	
	Jaguar *leftMotor;
	Jaguar *rightMotor;
	Jaguar *conveyorBeltMotor;
	Jaguar *turretMotor;
	Jaguar *topTurretWheel;
	Jaguar *bottomTurretWheel;
	
	Encoder *rightEncoder;
	Encoder *leftEncoder;

	GearTooth *gearToothCounter;
	
	Relay *rampTool;
	
	PIDController *leftMotorController;
	PIDController *rightMotorController;
	
	Solenoid *ringLight;
	
	RobotDrive *myRobot;
	
	Joystick *leftStick;
	Joystick *rightStick;
	Joystick *manipStick;
	
	Joystick*gameStick;
	
	Timer *timer;
	//Timer *autoTimer;
	Timer *utilityTimer;
	DriverStation *station;
	DriverStationLCD *screen;
	DriverStationEnhancedIO *enhancedIO;
	
	//image processing objects
	AxisCamera *camera;
	
	HSLImage *rawImage;
	MonoImage *monoImage;
	ColorImage *thresholdedImage;
	ColorImage *labeledImage;
	ColorImage *equalizedImage;
	ColorImage *convexedImage;
	ColorImage *sizeFilteredImage;

	//files for debugging, and to keep track of current image name
	FILE *debugFile;
	FILE *imageCounter;
	
	//DigitalInput *ballDetectLimit;
	DigitalInput *rightTurretLimit;
	DigitalInput *leftTurretLimit;
	DigitalInput *rampToolStop;
	
	
	//variables
	float pickUpSpeed;
	float turretValue;
	float topWheelValue;
	float bottomWheelValue;
	float leftValue;
	float rightValue;
	float correctedLeftValue;
	float correctedRightValue;
	
	//These keep track of whether or not a button is currently pushed
	bool previousTurretFire;
	bool previousWheelSpeedIncrement;
	bool previousWheelSpeedDecrement;
	float crouch1;
	float crouch2;
	Relay::Value previousRampToolDirection;
	
	//Limit switch values
	bool ballLimit;
	bool rightLimit;
	bool leftLimit;

	//Vision and control loop variables
	int reportCount;
	double distance;
	double rotAngle;
	double prevTime;
	double robotAngle;
	double turretAngle;
	double error;
	double potValue;
	bool  manualControl;
	bool  autoTurn;
	fieldSide side;
	
// Speed Control stuff
	double tIntial;
	int xIntial;
	bool newInterval;
	double currentSpeed;
	
#ifdef INTAKE_CONTROL	
	// Intake stuff
	Compressor *compressor;
	DigitalInput *proximtySensorTop;
	DigitalInput *proximtySensorBottom;
	Solenoid *solenoidTopOn;
	Solenoid *solenoidTopOff;
	Solenoid *solenoidBottomOn;
	Solenoid *solenoidBottomOff;
	bool topRetract;
	bool botRetract;
	bool runningIntake;
#endif
	
	//data structures
	vector<particlePoint> *particles;
	particlePoint currentPoint;
	
	Ballistic2D *bo;
public:
	Robot2489();
	void Autonomous();
	void OperatorControl();
	void ConveyorBelt();

	void InitJaguar();
	void InitEncoders();
	void InitSpike();
	void InitPIDControllers();
	void InitSolenoid();
	void InitJoystick();
	void InitIntake();

	void ControlConveyorBelt();
	void ControlTurretTurn();
	void ControlTurretFiring();
	void ControlRampTool();
	void TurnTurretAuton();
	void FireTurretAuton();
	void CorrectDrive();
	void ResetTurret();
	void TurnAuton(double angle);
	void MoveAuton(double distance);
	void TimeTurn(double time);
	void TimeMove(double time);
	//void MoveAutonSpeedControl();
	void StartAutoAimTurret();
	void UpdateSpeeds();
	void InitLimitSwitch();
	double LogisticSpeedProgression(double jpos);
	void DisplayGearToothVelocity();
	void UpdateGearToothVelocity();
	
	//Vision stuff
	void InitImage();

	void TestImages();
	void TestSingleImage(string);
	void RecordCurrentImage();
	void StoreParticles();
	void OrderParticles();
	void FilterParticles();

	void DetectRectangles();
	void InitializeImages();
	void DeleteImages();
	void ProcessLocation();
	void changeType(ColorImage *image, ImageType type);
	particlePoint AccessParticle(particlePos position);
	
	//Intake code
	void ControlIntake();
	
	//Hall effect
	void ControlGearTooth();
	
	
};

